/*
Digital Image Processing - Final Project
9 December 2011

Joseph Dombrowski
Josh Dixon
Junmo An
*/

#include "image.h"

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Constructors
Image::Image()
{}
Image::Image(string filename)
{
	image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_ANYCOLOR);

	int index=0;
	if(filename.find_last_of('\\') != filename.npos)
		index = filename.find_last_of('\\') + 1;

	windowName = filename.substr(index,filename.find_last_of('.')-index);
	initializeImage();
	
	// init parameters
	centerOfDistortion = pointD(image->width/2, image->height/2);
	s_aspectRatio = 1.0;
	longEdge = (image->width > image->height ? image->width : image->height);
	
	double radius = pow(pow(1.0*image->width/s_aspectRatio,2.0) + pow(1.0*image->height,2.0), 0.5);
	pointD fixedCenter = pointD(image->width/2, image->height/2);
	double min = -1/pow(radius,2.0);
	double max = (longEdge-1)/pow(radius,2.0);
	k_distortionFactor = min + (max-min)/2;
	
	
}
Image::Image(string filename, string windowname)
{
	image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_ANYCOLOR);
	windowName = windowname;
	initializeImage();
	
	// init parameters
	centerOfDistortion = pointD(image->width/2, image->height/2);
	s_aspectRatio = 1.0;
	longEdge = (image->width > image->height ? image->width : image->height);
	
	double radius = pow(pow(1.0*image->width/s_aspectRatio,2.0) + pow(1.0*image->height,2.0), 0.5);
	pointD fixedCenter = pointD(image->width/2, image->height/2);
	double min = -1/pow(radius,2.0);
	double max = (longEdge-1)/pow(radius,2.0);
	k_distortionFactor = min + (max-min)/2;
}
Image::Image(IplImage* inputImage, string windowname, double k, pointD center, double s)
{
 	image = copyImg(inputImage);
	windowName = windowname;
	initializeImage();
	
	// set parameters from the passed in values
	k_distortionFactor = k;
	centerOfDistortion = center;
	s_aspectRatio = s;
}
Image::Image(vector<vector<edgePoint>> edgeMatrix, string windowname)
{
	// Size image to match
	image = cvCreateImage(cvSize(edgeMatrix.size(),edgeMatrix[0].size()),8,1);

	// Copy image
	for(int y=0; y<image->height; y++)
	{
		for(int x=0; x<image->width; x++)
		{
			for(int c=0; c<image->nChannels; c++)
			{
				double mag = edgeMatrix[x][y].magnitude;
				if(mag > 255)
					mag = 255;
				Image::setImgPoint(image,point(x,y),c,mag);
			}
		}
	}

	windowName = windowName;
	initializeImage();
}
Image::Image(Image &source, string imagename)
{
	int w = source.width();
	int h = source.height();
	int d = source.depth();
	int ch = source.channels();

	IplImage* image = cvCreateImage(cvSize(w,h),d,ch);

	for(int y=0; y<h; y++)
		for(int x=0; x<w; x++)
			for(int c=0; c<ch; c++)
				Image::setImgPoint(image,point(x,y),c,source.getImgPoint(point(x,y),c));
	
	windowName = imagename;
	initializeImage();
}
void Image::initializeImage()
{
	edgeMap.resize(image->width);
	for(int i=0; i<image->width; i++)
		edgeMap[i].resize(image->height);

	clearMaps();

	initialized[_IMAGE] = true;
	initialized[_HISTOGRAM] = false;
	initialized[_EDGES] = false;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Destructor and Release
Image::~Image()
{
	imgEdges.clear();
	imgHist.clear();
	for(int i=0; i<edgeMap.size(); i++)
		edgeMap[i].clear();
	if(edgeMap.size() > 0)
		edgeMap.clear();
}
void Image::releaseImage()
{
	cvDestroyWindow(windowName.c_str());
	cvReleaseImage(&image);
}
void Image::clearMaps()
{
	for(int y=0; y<edgeMap[0].size(); y++)
	{
		for(int x=0; x<edgeMap.size(); x++)
		{
			edgeMap[x][y].direction = 0;
			edgeMap[x][y].magnitude = 0;
		}
	}
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Display
void Image::showImage()
{
	if(initialized[0])
	{
		cvNamedWindow(windowName.c_str());
		cvShowImage(windowName.c_str(), image);
		cvWaitKey(50);
	}
	else
		cout << "Image not initialized, will not be displayed." << endl;
}
void Image::hideImage()
{
	cvDestroyWindow(windowName.c_str());
}
void Image::showHistogram()
{
	// Calculate the histogram if it's not already
	if(!initialized[_HISTOGRAM])
		histogram();

	// Vector for the normalized histogram
	vector<int> normHist;
	normHist.resize(imgHist.size());

	int max = INT_MIN;

	// Find the max value in the histogram
	for(int i=0; i<imgHist.size(); i++)
		if(imgHist[i] > max)
			max = imgHist[i];

	// Fill the normalized histogram
	for(int i=0; i<imgHist.size(); i++)
		normHist[i] = (int)(((double)imgHist[i]/(double)max) * 100.0);

	// Make a small image for the histogram, initialize black
	IplImage* dispHist = cvCreateImage(cvSize(normHist.size(),100),8,1);
	for(int y=0; y<dispHist->height; y++)
		for(int x=0; x<dispHist->width; x++)
			Image::setImgPoint(dispHist,point(x,y),0);

	// Draw normalized histogram
	for(int x=0; x<dispHist->width; x++)
	{
		for(int y=dispHist->height-1; y>(dispHist->height-1) - normHist[x]; y--)
		{
			Image::setImgPoint(dispHist,point(x,y),255);
		}
	}

	// Display
	string hist = "Histogram ";
	string wName = hist + windowName;

	cvNamedWindow(wName.c_str());
	cvShowImage(wName.c_str(), dispHist);
	cvWaitKey(50);
	cvReleaseImage(&dispHist);
}
void Image::showHistogram(Image& theImage) // Draws the histogram on the sidebar
{
	// Calculate histogram if it isn't already
	if(!initialized[_HISTOGRAM])
		histogram();

	vector<int> normHist;
	normHist.resize(imgHist.size());

	int max = INT_MIN;

	// Find max value
	for(int i=0; i<imgHist.size(); i++)
		if(imgHist[i] > max)
			max = imgHist[i];

	// Normalize histogram
	for(int i=0; i<imgHist.size(); i++)
		normHist[i] = (int)(((double)imgHist[i]/(double)max) * 110.0);

	// Initialize small image for histogram
	IplImage* dispHist = cvCreateImage(cvSize(normHist.size(),110),8,3);
	for(int y=0; y<dispHist->height; y++)
		for(int x=0; x<dispHist->width; x++)
			Image::setImgPoint(dispHist,point(x,y),255);

	// Draw the histogram
	for(int x=0; x<dispHist->width; x++)
		for(int y=dispHist->height-1; y>(dispHist->height-1) - normHist[x]; y--)
			Image::setImgPoint(dispHist,point(x,y),0);

	// Copy it to the sidebar and release
	theImage.drawImgAt(dispHist,point(22,95));
	theImage.showImage();
	cvReleaseImage(&dispHist);
}
void Image::hideHistogram() // Kills the histogram window
{
	string hist = "Histogram ";
	string wName = hist + windowName;
	cvDestroyWindow(wName.c_str());
}
void Image::showEdges() // Displays an image of the member image's edges
{
	if(initialized[_EDGES])
	{
		IplImage* thisThing = cvCreateImage(cvSize(image->width,image->height),8,1);
		
		// Initialize image to black
		for(int y=0; y<thisThing->height; y++)
			for(int x=0; x<thisThing->width; x++)
				setImgPoint(thisThing,point(x,y),0);

		// Fill it with edge data
		for(int j=0; j<imgEdges.size(); j++)
			for(int i=0; i<imgEdges[j].points.size(); i++)
				setImgPoint(thisThing,point(imgEdges[j].points[i].x,imgEdges[j].points[i].y),255);

		// Display and release
		string s = "Edges of ";
		string wName = s + windowName;

		showImage(thisThing,wName.c_str());
		cvReleaseImage(&thisThing);
	}
}
void Image::hideEdges() // Hide the edge window
{
	string s = "Edges of ";
	string wName = s + windowName;

	cvDestroyWindow(wName.c_str());
}


void Image::showImage(IplImage* theImage, string windowName) // Static function to display an image
{
	cvNamedWindow(windowName.c_str());
	cvShowImage(windowName.c_str(), theImage);
	cvWaitKey(50);
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Access Data
unsigned char Image::getImgPoint(point p, int channel)
{
	if(p.x < 0 || p.x >= image->width || p.y < 0 || p.y >= image->height || !initialized[_IMAGE])
		return 0; 
	return (int)image->imageData[p.y*image->widthStep + p.x*image->nChannels + channel];
}
unsigned char Image::getImgPoint(point p)
{
	if(p.x < 0 || p.x >= image->width || p.y < 0 || p.y >= image->height || !initialized[_IMAGE])
		return 0; 
	int value = 0;
	for(int c=0; c<image->nChannels; c++)
	{
		value += image->imageData[p.y*image->widthStep + p.x*image->nChannels + c];
	}
	value = value/image->nChannels;

	return (unsigned char)value;
}
unsigned char Image::getImgPoint(pointD p) // Return interpolated point
{
	// Look at the 4 points it takes values from
	int p1[] = {floor(p.x),floor(p.y)}; // Lower left
	int p2[] = {ceil(p.x),floor(p.y)}; // Lower right
	int p3[] = {floor(p.x),ceil(p.y)}; // Upper left
	int p4[] = {ceil(p.x),ceil(p.y)}; // Upper right

	// Find the char values at each point
	unsigned char p1V = getImgPoint(image,point(p1[0],p1[1]));
	unsigned char p2V = getImgPoint(image,point(p2[0],p2[1]));
	unsigned char p3V = getImgPoint(image,point(p3[0],p3[1]));
	unsigned char p4V = getImgPoint(image,point(p4[0],p4[1]));
	
	// Find the weights
	double p1W = (floor(p.x+1)-p.x)  * (floor(p.y+1)-p.y);
	double p2W = (p.x-floor(p.x)) * (floor(p.y+1)-p.y);
	double p3W = (floor(p.x+1)-p.x)  * (p.y-floor(p.y));
	double p4W = (p.x-floor(p.x)) * (p.y-floor(p.y));

	// Normalize the weights
	double tW = p1W + p2W + p3W + p4W;
	p1W = p1W/tW;	p2W = p2W/tW;	p3W = p3W/tW;	p4W = p4W/tW;	
	
	// Average the 4 values and return
	return int((p1W*p1V) + (p2W*p2V) + (p3W*p3V) + (p4W*p4V));
}

// Static versions of the same functions
unsigned char Image::getImgPoint(IplImage* source, point p, int channel)
{
	if(p.x < 0 || p.x >= source->width || p.y < 0 || p.y >= source->height)
		return 0;
	return source->imageData[p.y*source->widthStep + p.x*source->nChannels + channel];
}
unsigned char Image::getImgPoint(IplImage* source, point p)
{
	if(p.x < 0 || p.x >= source->width || p.y < 0 || p.y >= source->height)
		return 0; 
	int value = 0;
	for(int c=0; c<source->nChannels; c++)
	{
		value += source->imageData[p.y*source->widthStep + p.x*source->nChannels + c];
	}
	value = value/source->nChannels;

	return value;

}

// Get data about the image
double Image::getK()
{
	return k_distortionFactor;
}
double Image::getS()
{
	return s_aspectRatio;
}
pointD Image::getCenter()
{
	return centerOfDistortion;
}
int Image::width()
{
	if(!initialized[_IMAGE])
		return 0;
	return image->width;
}
int Image::height()
{
	if(!initialized[_IMAGE])
		return 0;
	return image->height;
}
int Image::channels()
{
	if(!initialized[_IMAGE])
		return 0;
	return image->nChannels;
}
int Image::depth()
{
	if(!initialized[_IMAGE])
		return 0;
	return image->depth;
}
string Image::name()
{
	if(!initialized[_IMAGE])
		return 0;
	return windowName;
}

// Returns the average intensity of the member image
unsigned char Image::average()
{
	if(!initialized[_IMAGE])
		return 0;

	int value = 0;

	for(int y=0; y<image->height; y++)
	{
		for(int x=0; x<image->width; x++)
		{
			value += (int)getImgPoint(point(x,y));
		}
	}

	value = value / (image->width*image->height);

	return value;
}

// Returns a copy of the image
IplImage* Image::copyImg()
{	
	IplImage* result = cvCreateImage(cvSize(image->width,image->height),image->depth,image->nChannels);
	for(int y=0; y<image->height; y++)
	{
		for(int x=0; x<image->width; x++)
		{
			for(int c=0; c<image->nChannels; c++)
			{
				Image::setImgPoint(result,point(x,y),c,getImgPoint(image,point(x,y),c));
			}
		}
	}
	return result;
}
IplImage* Image::copyImg(IplImage* source)
{	
	IplImage* result = cvCreateImage(cvSize(source->width,source->height),source->depth,source->nChannels);
	for(int y=0; y<source->height; y++)
		for(int x=0; x<source->width; x++)
			for(int c=0; c<source->nChannels; c++)
				Image::setImgPoint(result,point(x,y),c,Image::getImgPoint(source,point(x,y),c));
	return result;
}

// Saves the image
void Image::saveImage(string fName)
{
	cvSaveImage(fName.c_str(),image);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Simple Manipulation
void Image::setImgPoint(point p, int channel, unsigned char value)
{
	if(!initialized[_IMAGE])
		return;
	if(p.x >= 0 && p.x < image->width && p.y >= 0 && p.y < image->height)
	{
		if(channel <= image->nChannels)
			image->imageData[p.y*image->widthStep + p.x*image->nChannels + channel] = value;
		else
			image->imageData[p.y*image->widthStep + p.x*image->nChannels + image->nChannels] = value;
	}
}
void Image::setImgPoint(point p, unsigned char v)
{
	if(!initialized[_IMAGE])
		return;
	if(p.x >= 0 && p.x < image->width && p.y >= 0 && p.y < image->height)
	{
		for(int c=0; c<image->nChannels; c++)
			setImgPoint(p, c, v);
	}
}
void Image::setImgPoint(point p, CvScalar color)
{
	if(!initialized[_IMAGE])
		return;
	if(p.x >= 0 && p.x < image->width && p.y >= 0 && p.y < image->height)
	{
		for(int c=0; c<image->nChannels; c++)
			setImgPoint(p, c, color.val[c]);
	}
}
void Image::setImgPoint(point p, unsigned char red, unsigned char green, unsigned char blue)
{
	if(!initialized[_IMAGE])
		return;
	if(p.x >= 0 && p.x < image->width && p.y >= 0 && p.y < image->height)
	{
		CvScalar color; color.val[R] = red; color.val[G] = green; color.val[B] = blue;
		for(int c=0; c<image->nChannels; c++)
			setImgPoint(p, c, color.val[c]);
	}
}

// Static versions of the same functions
void Image::setImgPoint(IplImage* result, point p, int channel, unsigned char value)
{
	if(p.x >= 0 && p.x < result->width && p.y >= 0 && p.y < result->height)
	{
		if(channel <= result->nChannels)
			result->imageData[p.y*result->widthStep + p.x*result->nChannels + channel] = value;
		else
			result->imageData[p.y*result->widthStep + p.x*result->nChannels + result->nChannels] = value;
	}
}
void Image::setImgPoint(IplImage* result, point p, unsigned char v)
{
	if(p.x >= 0 && p.x < result->width && p.y >= 0 && p.y < result->height)
	{
		for(int c=0; c<result->nChannels; c++)
			Image::setImgPoint(result, p, c, v);
	}
}
void Image::setImgPoint(IplImage* result, point p, CvScalar color)
{
	if(p.x >= 0 && p.x < result->width && p.y >= 0 && p.y < result->height)
	{
		for(int c=0; c<result->nChannels; c++)
			Image::setImgPoint(result, p, c, color.val[c]);
	}
}
void Image::setImgPoint(IplImage* result, point p, unsigned char red, unsigned char green, unsigned char blue)
{
	if(p.x >= 0 && p.x < result->width && p.y >= 0 && p.y < result->height)
	{
		CvScalar color; color.val[R] = red; color.val[G] = green; color.val[B] = blue;
		for(int c=0; c<result->nChannels; c++)
			Image::setImgPoint(result, p, c, color.val[c]);
	}
}

// Color an entire image
void Image::setWhite()
{
	if(initialized[_IMAGE])
		for(int x = 0; x < image->width; x++)
			for(int y = 0; y < image->height; y++)
				for(int c=0; c<image->nChannels; c++)
					setImgPoint(point(x,y), c, pow(2.0,image->depth)-1);
}
void Image::setColor(CvScalar color)
{
	if(initialized[_IMAGE])
		for(int x = 0; x < image->width; x++)
			for(int y = 0; y < image->height; y++)
				setImgPoint(point(x,y), color);
}
void Image::setColor(unsigned char intensity)
{
	if(initialized[_IMAGE])
		for(int x = 0; x < image->width; x++)
			for(int y = 0; y < image->height; y++)
				for(int c=0; c<image->nChannels; c++)
					setImgPoint(point(x,y), c, intensity);
}
void Image::setColor(unsigned char red, unsigned char green, unsigned char blue)
{
	if(initialized[_IMAGE])
		for(int x = 0; x < image->width; x++)
			for(int y = 0; y < image->height; y++)
				setImgPoint(point(x,y),red,green,blue);
}

void Image::setWhite(IplImage* result)
{
	for(int x = 0; x < result->width; x++)
		for(int y = 0; y < result->height; y++)
			Image::setImgPoint(result, point(x,y), pow(2.0,result->depth)-1);
}
void Image::setColor(IplImage* result, CvScalar color)
{
	for(int x = 0; x < result->width; x++)
		for(int y = 0; y < result->height; y++)
			Image::setImgPoint(result, point(x,y), color);
}
void Image::setColor(IplImage* result, unsigned char intensity)
{
	for(int x = 0; x < result->width; x++)
		for(int y = 0; y < result->height; y++)
			for(int c=0; c<result->nChannels; c++)
				Image::setImgPoint(result, point(x,y), intensity);
}
void Image::setColor(IplImage* result, unsigned char red, unsigned char green, unsigned char blue)
{
	for(int x = 0; x < result->width; x++)
		for(int y = 0; y < result->height; y++)
			Image::setImgPoint(result, point(x,y),red,green,blue);
}

// Draws text on the image at a specified location, with background
void Image::drawLabel(string message, point thePoint, CvScalar color, int _width, double _size)
{
	if(thePoint.x < 0 || thePoint.x >= image->width || thePoint.y+16 < 0 || thePoint.y+16 >= image->height)
		return;

	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, _size, _size, 0, 1.5, CV_AA);
	
	cvDrawRect(image,cvPoint(thePoint.x,thePoint.y),cvPoint(thePoint.x+_width,thePoint.y+22),cvScalar(255,255,255,1),CV_FILLED,CV_AA,0);
	cvPutText(image, message.c_str(), cvPoint(thePoint.x,thePoint.y+16), &font, color);

	showImage();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Image Processing

// Compute Histogram
int Image::histogram(vector<int>& histogram) // Returns median
{
	int size = pow(2.0,image->depth);
	histogram.resize(size);
	imgHist.resize(size);

	for(int i=0; i<histogram.size(); i++)
		histogram[i] = 0;

	// Look at all points, and for a given intensity, increment that histogram entry
	for(int y=0; y<image->height; y++)
		for(int x=0; x<image->width; x++)
			histogram[(int)getImgPoint(point(x,y))]++;

	// Copy it to member histogram
	for(int i=0; i<histogram.size(); i++)
		imgHist[i] = histogram[i];

	// Find median
	int max = INT_MIN;
	int value = 0;
	for(int i=0; i<histogram.size(); i++)
	{
		if(max < imgHist[i])
		{
			max = imgHist[i];
			value = i;
		}
	}

	initialized[_HISTOGRAM] = true;

	return value;
}
int Image::histogram() // Returns median
{
	int size = pow(2.0,image->depth);
	imgHist.resize(size);
	for(int i=0; i<imgHist.size(); i++)
		imgHist[i] = 0;

	// Count each intensity value, set histogram
	for(int y=0; y<image->height; y++)
		for(int x=0; x<image->width; x++)
			imgHist[getImgPoint(point(x,y))]++;

	int max = INT_MIN;
	int value = 0;

	// Find median
	for(int i=0; i<imgHist.size(); i++)
	{
		if(max < imgHist[i])
		{
			max = imgHist[i];
			value = i;
		}
	}

	initialized[_HISTOGRAM] = true;

	return value;
}

// Saves thresholded image to 'result'
void Image::thresholdImg(IplImage* result, int thresh)
{
	if(image->width != result->width || image->height != result->height || !initialized[_IMAGE])
		return;
	int value=0;
	int nChan = result->nChannels;

	for(int y=0; y<image->height; y++)
	{
		for(int x=0; x<image->width; x++)
		{
			value = (int)getImgPoint(image,point(x,y));
			if(value < thresh)
				for(int c=0; c<nChan; c++)
					Image::setImgPoint(result,point(x,y),c,255);
			else
				for(int c=0; c<nChan; c++)
					Image::setImgPoint(result,point(x,y),c,0);
		}
	}
}
// Overwrites image with binarized image
void Image::thresholdImg(int thresh)
{
	if(!initialized[_IMAGE])
		return;

	int value=0;
	int nChan = image->nChannels;

	for(int y=0; y<image->height; y++)
	{
		for(int x=0; x<image->width; x++)
		{
			value = (int)getImgPoint(image,point(x,y));
			if(value < thresh)
				for(int c=0; c<nChan; c++)
					Image::setImgPoint(image,point(x,y),c,255);
			else
				for(int c=0; c<nChan; c++)
					Image::setImgPoint(image,point(x,y),c,0);
		}
	}
}
// Overlays an image on anther image
void Image::drawImgAt(IplImage* source, point p)
{
	if(!initialized[_IMAGE])
		image = cvCreateImage(cvSize(source->width+p.x,source->height+p.y),source->depth,source->nChannels);

	int nChan = image->nChannels;
	if(source->nChannels < nChan) nChan = source->nChannels;

	int yMax = p.y+source->height;
	if(yMax >= image->height) yMax = image->height;

	int xMax = p.x+source->width;
	if(xMax >= image->width) xMax = image->width;

	for(int y=p.y; y<yMax; y++)
	{
		for(int x=p.x; x<xMax; x++)
		{
			for(int c=0; c<nChan; c++)
			{
				setImgPoint(point(x,y),c,getImgPoint(source,point(x-p.x,y-p.y),c));
			}
		}
	}
}
// Returns an image that is the member image padded with 0's
IplImage* Image::zeroPad(int top, int right, int bottom, int left)
{
	IplImage* result = cvCreateImage(cvSize(image->width+left+right,image->height+top+bottom),image->depth,image->nChannels);
	for(int y=0; y<result->height; y++)
	{
		for(int x=0; x<result->width; x++)
		{
			if(y<top || y>=top+image->height || x<left || x>=left+image->width)
				Image::setImgPoint(result,point(x,y),0);
			else
				Image::setImgPoint(result,point(x,y),getImgPoint(image,point(x,y)));
		}
	}
	return result;
}

IplImage* Image::zeroPad(IplImage* source, int top, int right, int bottom, int left)
{
	IplImage* result = cvCreateImage(cvSize(source->width+left+right,source->height+top+bottom),source->depth,source->nChannels);
	for(int y=0; y<result->height; y++)
	{
		for(int x=0; x<result->width; x++)
		{
			if(y<top || y>=top+source->height || x<left || x>=left+source->width)
				Image::setImgPoint(result,point(x,y),0);
			else
				Image::setImgPoint(result,point(x,y),getImgPoint(source,point(x,y)));
		}
	}
	return result;
}

// Smooths image, Average or Gaussian, of any size
void Image::smoothImg(int type)
{
	smoothImg(type,5);
}
void Image::smoothImg(int type, int size)
{
	IplImage* result = Image::copyImg(image);
	int value;

	if(size%2 != 1) // Window must be odd sized
		size++;


	switch(type)
	{
	case _AVERAGE :
		for(int y=0; y<result->height; y++)
		{
			for(int x=0; x<result->width; x++)
			{
				value = 0;
				for(int j=0; j<size; j++)
					for(int i=0; i<size; i++)
						value += (int)getImgPoint(image,point(x+i-((size-1)/2),y+j-((size-1)/2))); // Add up all the points

				value = (double)value/(size*size); // Average them
				Image::setImgPoint(result,point(x,y),(int)value);
			}
		}
		break;

	case _GAUSSIAN :
		vector<vector<double>> gaussian;

		gaussian.resize(size);
		for(int i=0; i<gaussian.size(); i++)
			gaussian[i].resize(size);

		int s = (size-1)/2;
		double o = 0.19*(size/1);

		// Fill gaussian matrix
		for(int y=0; y<size; y++)
			for(int x=0; x<size; x++)
				gaussian[x][y] = (1/(2 * _PI * (o*o))) * pow(_E,(-((x-s)*(x-s)+(y-s)*(y-s)))/(2 * (o*o)));

		for(int y=0; y<result->height; y++)
		{
			for(int x=0; x<result->width; x++)
			{
				value=0;

				for(int j=0; j<size; j++)
					for(int i=0; i<size; i++)
						value = value + (gaussian[i][j] * (int)getImgPoint(image,point(x+i-s,y+j-s))); // Add gaussian weighted value

				Image::setImgPoint(result,point(x,y),value);
			}
		}
		break;
	}

	image = copyImg(result);
}

// Find the edges, and fill edge vectors
void Image::detectEdges(vector<edge>& edges)
{
	// Copy original image to process
	Image smoothed(copyImg(image),"Smoothed", k_distortionFactor, centerOfDistortion, s_aspectRatio);
	smoothed.smoothImg(_GAUSSIAN,5);		// Apply gaussian smoothing
	clearMaps();							// Clear the edge matrix
	smoothed.mapEdges(edgeMap);				// Fill the edge matrix
	suppressEdges(edgeMap);					// Suppress non-maxima
	double thresh = getThresh(edgeMap);		// Find threshold
	threshold(edgeMap,thresh/2.0,thresh);	// Apply threshold and hysterisis tracking

	// Now we take the edges, and we push them to an array
	int edgeNum = 0;
	bool moreEdges = false;
	do
	{
		moreEdges = false;
		
		for(int y=0; y<edgeMap[0].size(); y++)
		{
			for(int x=0; x<edgeMap.size(); x++)
			{
				if(edgeMap[x][y].magnitude == 255)
				{
					edges.resize(edgeNum+1);				// Make room for an edge
					pushPoint(edgeNum,point(x,y),edges);	// Push the points in that edge
					x = edgeMap.size();						// End for loop
					y = edgeMap[0].size();
					
					edgeNum++;								// Increment current edge number
					moreEdges = true;						// Go again
				}
			}
		}
	}while(moreEdges);
	
	// Erase short edges
	int i=0;
	int diag = Common::getDist(point(0,0),point(image->width-1,image->height-1));
	
	while(i < edges.size())
	{
		// Length is the diagonal length
		double length = Common::getDist(point(edges[i].points[0].x,edges[i].points[0].y),point(edges[i].points[edges[i].points.size()-1].x,edges[i].points[edges[i].points.size()-1].y));
		if(length < ((double)diag / 14.0))
			edges.erase(edges.begin()+i);
		else
			i++;
	}
	
	// Figure out the angle of the edges
	for(int i=0; i<edges.size(); i++)
	{
		int bX, eX, bY, eY;

		bX = edges[i].points[0].x; // Begin points
		bY = edges[i].points[0].y;

		eX = edges[i].points[edges[i].points.size()-1].x; // End points
		eY = edges[i].points[edges[i].points.size()-1].y;

		edges[i].angle = (int)((atan((double)(eY-bY)/(double)(eX-bX)) * 180 / _PI)+180)%180;
	}
	
	mergeEdges(edges); // Merge similar edges

	for(int i=0; i<edges.size(); i++) // Copy edges to member vector
		imgEdges.push_back(edges[i]);

	//Uncomment section to show the final detected edges
	//IplImage* thisThing = cvCreateImage(cvSize(image->width,image->height),8,1);
	//
	//for(int y=0; y<thisThing->height; y++)
	//	for(int x=0; x<thisThing->width; x++)
	//		setImgPoint(thisThing,point(x,y),0);
	//for(int j=0; j<edges.size(); j++)
	//{
	//	for(int i=0; i<edges[j].points.size(); i++)
	//		setImgPoint(thisThing,point(edges[j].points[i].x,edges[j].points[i].y),255);
	//	showImage(thisThing);
	//	cvWaitKey(0); // Comment this line to see all at once
	//}

	smoothed.releaseImage(); // Destroy image

	initialized[_EDGES] = true;
}

// The next group of functions calculates and minimizes distortion
double Image::calculateDistortion()
{
	return calculateDistortion(centerOfDistortion, k_distortionFactor, s_aspectRatio);
}
double Image::calculateDistortion(pointD centerOfDistortion, double k, double s)
{
	if(!initialized[_EDGES] || !initialized[_IMAGE])
		return -1;

	vector<edge> tempEdges;
	tempEdges.resize(imgEdges.size());

	int Xu, Yu, Xd, Yd, Rd;

	// Make the corrected edge map given the input parameters
	for(int i=0; i<imgEdges.size(); i++)
	{
		for(int j=0; j<imgEdges[i].points.size(); j++)
		{
			Xd = imgEdges[i].points[j].x;
			Yd = imgEdges[i].points[j].y;
			Rd = sqrt( pow(((Xd - centerOfDistortion.x)/s),2) + pow((Yd - centerOfDistortion.y),2) );

			Xu = Xd + (Xd - centerOfDistortion.x)*(k * (Rd*Rd));
			Yu = Yd + (Yd - centerOfDistortion.y)*(k * (Rd*Rd));

			tempEdges[i].points.push_back(point(Xu,Yu));
		}
	}

	// Calculate the distortion
	double distortion = 0;
	for(int i=0; i<tempEdges.size(); i++)
	{
		int bX, eX, bY, eY;

		bX = tempEdges[i].points[0].x;
		bY = tempEdges[i].points[0].y;

		eX = tempEdges[i].points[tempEdges[i].points.size()-1].x;
		eY = tempEdges[i].points[tempEdges[i].points.size()-1].y;
		
		Line ideal(pointD(bX,bY),pointD(eX,eY));

		for(int j=0; j< tempEdges[i].points.size(); j++)
		{
			distortion += ideal.distFrom(pointD(tempEdges[i].points[j].x,tempEdges[i].points[j].y));
		}
	}

	return distortion;
}
void Image::minimizeDistortionByAdjustingK()
{
	if(!initialized[_IMAGE] || !initialized[_EDGES])
	{
		cout << "Initialize image and edges before calculating k." << endl;
		return;
	}

	// Initialize values
	double radius = pow(pow(1.0*image->width,2.0) + pow(1.0*image->height,2.0), 0.5);
	double min = -1/pow(radius,2.0);
	double max = (longEdge-1)/pow(radius,2.0);
	double distortionError[2] ={0.0, 0.0}; // Hooray - distortion is 0!
	double kAdjustment = 1.0;
	double threshold = .000000000001;

	while(kAdjustment >= threshold)
	{
		// Split the range being considered into 2 subranges and find the middle of each subrange
		double testLow = (max - min)*0.25 + min;
		double testHigh = (max - min)*0.75 + min;

		// Calculate distortion
		distortionError[0] = calculateDistortion(centerOfDistortion, testLow, s_aspectRatio);
		distortionError[1] = calculateDistortion(centerOfDistortion, testHigh, s_aspectRatio);
		
		// Which side is smaller
		int minKrange= (distortionError[0] < distortionError[1] ? 0 : 1);
		
		// Choose smaller distortion, and move to that half
		switch(minKrange)
		{
		case 0:		// top left
			kAdjustment = abs(max - ((max - min)*0.5 + min));
			max = (max - min)*0.5 + min;
			break;
		case 1:		// top right
			kAdjustment = abs(min - ((max - min)*0.5 + min));
			min = (max - min)*0.5 + min;
			break;
		default:
			break;
		}
	}
	
	k_distortionFactor = (max - min)*0.5 + min; // Calculate K factor
}
void Image::minimizeDistortionByAdjustingCenterOfDistortion()
{
	if(!initialized[_IMAGE] || !initialized[_EDGES])
	{
		cout << "Initialize image and edges before calculating k." << endl;
		return;
	}
	
	// Initialize beginning values
	pointD min = pointD(0.0, 0.0);
	pointD max = pointD(image->width, image->height);
	double distortionError[4] ={0.0, 0.0, 0.0, 0.0}; // Hooray - distortion is 0!
	pointD centerAdjustment = pointD(1.0, 1.0);
	double threshold = 1.0;

	while(centerAdjustment.x >= threshold || centerAdjustment.y >= threshold)
	{
		double testMinX = (max.x - min.x)*0.25 + min.x;
		double testMaxX = (max.x - min.x)*0.75 + min.x;
		double testMinY = (max.y - min.y)*0.25 + min.y;
		double testMaxY = (max.y - min.y)*0.75 + min.y;

		// Split the region being considered into 4 subregions and find the middle of each subregion
		pointD topLeft = pointD(testMinX, testMinY);
		pointD topRight = pointD(testMaxX, testMinY);
		pointD bottomLeft = pointD(testMinX, testMaxY);
		pointD bottomRight = pointD(testMaxX, testMaxY);

		// Calculate the distortion of the regions
		distortionError[0] = calculateDistortion(topLeft, k_distortionFactor, s_aspectRatio);
		distortionError[1] = calculateDistortion(topRight, k_distortionFactor, s_aspectRatio);
		distortionError[2] = calculateDistortion(bottomLeft, k_distortionFactor, s_aspectRatio);
		distortionError[3] = calculateDistortion(bottomRight, k_distortionFactor, s_aspectRatio);

		// Find the correct quadrant
		int minDistortionErrorRegion=0;
		for(int i=1; i<4; i++)
		{
			if(distortionError[i] < distortionError[minDistortionErrorRegion])
				minDistortionErrorRegion=i;
		}

		// Move to that location and loop
		switch(minDistortionErrorRegion)
		{
		case 0:		// top left
			centerAdjustment = pointD(abs(max.x - ((max.x - min.x)*0.5 + min.x)), abs(max.y - ((max.y - min.y)*0.5 + min.y)));
			max.x = (max.x - min.x)*0.5 + min.x;
			max.y = (max.y - min.y)*0.5 + min.y;
			break;
		case 1:		// top right
			centerAdjustment = pointD(abs(min.x - ((max.x - min.x)*0.5 + min.x)), abs(max.y - ((max.y - min.y)*0.5 + min.y)));
			min.x = (max.x - min.x)*0.5 + min.x;
			max.y = (max.y - min.y)*0.5 + min.y;
			break;
		case 2:		// bottom left
			centerAdjustment = pointD(abs(max.x - ((max.x - min.x)*0.5 + min.x)), abs(min.y - ((max.y - min.y)*0.5 + min.y)));
			max.x = (max.x - min.x)*0.5 + min.x;
			min.y = (max.y - min.y)*0.5 + min.y;
			break;
		case 3:		// bottom right
			centerAdjustment = pointD(abs(min.x - ((max.x - min.x)*0.5 + min.x)), abs(min.y - ((max.y - min.y)*0.5 + min.y)));
			min.x = (max.x - min.x)*0.5 + min.x;
			min.y = (max.y - min.y)*0.5 + min.y;
			break;
		default:
			break;
		}
	}
	
	centerOfDistortion = pointD(((max.x - min.x)*0.5 + min.x), (max.y - min.y)*0.5 + min.y);
}
void Image::minimizeDistortionByAdjustingS()
{
	/*
	We were not able to find s. Once K and C are found, increasing s will always seem to make the distortion
	go down, but obviously that is not true. For this project we assume a value of 1, as per most online 
	resources.
	*/
	if(!initialized[_IMAGE] || !initialized[_EDGES])
	{
		cout << "Initialize image and edges before calculating k." << endl;
		return;
	}

	double radius = pow(pow(1.0*image->width,2.0) + pow(1.0*image->height,2.0), 0.5);
	pointD fixedCenter = pointD(image->width/2, image->height/2);

	double min = -100;
	double max = 100;
	
	double distortionError[2] ={0.0, 0.0}; // Hooray - distortion is 0!

	double adjustment = 1.0;

	double tolerance = .0001;
	while(adjustment >= tolerance)
	{
		// split the range being considered into 2 subranges and find the middle of each subrange
		double testLow = (max - min)*0.25 + min;
		double testHigh = (max - min)*0.75 + min;

		distortionError[0] = calculateDistortion(fixedCenter, k_distortionFactor, testLow);
		distortionError[1] = calculateDistortion(fixedCenter, k_distortionFactor, testHigh);
		

		int minRange= (distortionError[0] < distortionError[1] ? 0 : 1);
		
		switch(minRange)
		{
		case 0:		// top left
			adjustment = abs(max - ((max - min)*0.5 + min));
			max = (max - min)*0.5 + min;
			break;
		case 1:		// top right
			adjustment = abs(min - ((max - min)*0.5 + min));
			min = (max - min)*0.5 + min;
			break;
		default:
			break;
		}
	}
	
	s_aspectRatio = (max - min)*0.5 + min;
}
void Image::correctDistortion()
{
	if(!initialized[_IMAGE])
		return;

	// Copy original image
	Image distorted(image,"Distorted", k_distortionFactor, centerOfDistortion, s_aspectRatio);
	
	double Xd, Yd, Rd, Ru;
	pointD TL, TR, BL, BR;

	// Calculate the outer bounds for the corrected image
	Rd = sqrt( pow(((0 - centerOfDistortion.x)/s_aspectRatio),2) + pow((0 - centerOfDistortion.y),2) );
	TL.x = 0 + (0 - centerOfDistortion.x)*k_distortionFactor*(Rd*Rd);
	TL.y = 0 + (0 - centerOfDistortion.y)*k_distortionFactor*(Rd*Rd);

	Rd = sqrt( pow((((image->width-1) - centerOfDistortion.x)/s_aspectRatio),2) + pow((0 - centerOfDistortion.y),2) );
	TR.x = (image->width-1) + ((image->width-1) - centerOfDistortion.x)*k_distortionFactor*(Rd*Rd);
	TR.y = 0 + (0 - centerOfDistortion.y)*k_distortionFactor*(Rd*Rd);

	Rd = sqrt( pow(((0 - centerOfDistortion.x)/s_aspectRatio),2) + pow(((image->height-1) - centerOfDistortion.y),2) );
	BL.x = 0 + (0 - centerOfDistortion.x)*k_distortionFactor*(Rd*Rd);
	BL.y = (image->height-1) + ((image->height-1) - centerOfDistortion.y)*k_distortionFactor*(Rd*Rd);

	Rd = sqrt( pow((((image->width-1) - centerOfDistortion.x)/s_aspectRatio),2) + pow(((image->height-1) - centerOfDistortion.y),2) );
	BR.x = (image->width-1) + ((image->width-1) - centerOfDistortion.x)*k_distortionFactor*(Rd*Rd);
	BR.y = (image->height-1) + ((image->height-1) - centerOfDistortion.y)*k_distortionFactor*(Rd*Rd);

	// Find the minimum and maximum values for X and Y, mapped from distorted image
	int minX, maxX, minY, maxY;

	if(TL.x < BL.x)
		minX = TL.x;
	else
		minX = BL.x;

	if(TR.x > BR.x)
		maxX = TR.x;
	else
		maxX = BR.x;

	if(TL.y < TR.y)
		minY = TL.y;
	else
		minY = TR.y;

	if(BL.y > BR.y)
		maxY = BL.y;
	else
		maxY = BR.y;

	// Reinitialize image to be the new size
	cvReleaseImage(&image);
	image = cvCreateImage(cvSize(maxX-minX,maxY-minY), distorted.depth(), distorted.channels());
	Image::setColor(image,0);

	// Find the diagonal image size
	double diag = Common::getDist(point(0,0),point(image->width-1,image->height-1));

	for(int y=minY; y<=maxY; y++)
	{
		for(int x=minX; x<=maxX; x++)
		{
			// Ru is calculated using known values
			Ru = sqrt( pow(((x - centerOfDistortion.x)/s_aspectRatio),2) + pow((y - centerOfDistortion.y),2) );
			// Rd is calculated using a binary search for the minimizing value
			Rd = findRad(diag,k_distortionFactor,Ru);

			// Find the correct location on the distorted image, and set.
			Xd = (x + centerOfDistortion.x*k_distortionFactor*(Rd*Rd))/(1 + k_distortionFactor*(Rd*Rd));
			Yd = (y + centerOfDistortion.y*k_distortionFactor*(Rd*Rd))/(1 + k_distortionFactor*(Rd*Rd));

			setImgPoint(point(x-minX,y-minY),distorted.getImgPoint(pointD(Xd,Yd)));
		}
	}

	distorted.releaseImage(); // Destroy image
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Private functions

// Detect edges and fill matrix
void Image::mapEdges(vector<vector<edgePoint>>& edgeMatrix)
{
	// Apply Sobel operator
	int gradX[3][3] = {{-1,  0,  1},
					   {-2,  0,  2},
					   {-1,  0,  1}};

	int gradY[3][3] = {{ 1,  2,  1},
					   { 0,  0,  0},
					   {-1, -2, -1}};
	
	int vx,vy; // The magnitude in the two directions

	for(int y=0; y<image->height; y++)
	{
		for(int x=0; x<image->width; x++)
		{
			vx=0;vy=0;

			// Apply Sobel operator
			for(int j=0; j<3; j++)
			{
				for(int i=0; i<3; i++)
				{
					vx = vx + (gradX[i][j] * (int)getImgPoint(image,point(x+i-1,y+j-1)));
					vy = vy + (gradY[i][j] * (int)getImgPoint(image,point(x+i-1,y+j-1)));
				}
			}

			// Calculate magnatude and direction
			double mag = sqrt(pow(vy,2.0) + pow(vx,2.0));
			edgeMatrix[x][y].magnitude = mag;

			edgeMatrix[x][y].direction = (atan((double)vx/(double)vy) * 180 / _PI);
			if(edgeMatrix[x][y].direction < 0)
				edgeMatrix[x][y].direction = 360 + edgeMatrix[x][y].direction;
		}
	}
}
// Suppress non-maxima to get clean edges
void Image::suppressEdges(vector<vector<edgePoint>>& edgeMatrix)
{
	// Initialize temporary matrix
	vector<vector<edgePoint>> tempMap;
	tempMap.resize(edgeMatrix.size());

	for(int i=0; i<edgeMatrix.size(); i++)
		tempMap[i].resize(edgeMatrix[i].size());

	for(int y=0; y<edgeMatrix[0].size(); y++)
	{
		for(int x=0; x<edgeMatrix.size(); x++)
		{
			tempMap[x][y].magnitude = 0;
		}
	}

	// If the point is a maxima in its gradient direction, copy it to temp.
	for(int y=1; y<image->height-1; y++)
	{
		for(int x=1; x<image->width-1; x++)
		{
			int dir = (int)((edgeMatrix[x][y].direction/45.0)+0.5);

			switch(dir)
			{
				case 0 : 
				case 4 :
				case 8 : 
						if(edgeMatrix[x][y].magnitude >= edgeMatrix[x-1][y].magnitude && edgeMatrix[x][y].magnitude >= edgeMatrix[x+1][y].magnitude)
							tempMap[x][y].magnitude = edgeMatrix[x][y].magnitude;
					break;
				case 1 : 
				case 5 : 
					if(edgeMatrix[x][y].magnitude >= edgeMatrix[x+1][y-1].magnitude && edgeMatrix[x][y].magnitude >= edgeMatrix[x-1][y+1].magnitude)
						tempMap[x][y].magnitude = edgeMatrix[x][y].magnitude;
					break;
				case 2 : 
				case 6 : 
					if(edgeMatrix[x][y].magnitude >= edgeMatrix[x][y-1].magnitude && edgeMatrix[x][y].magnitude >= edgeMatrix[x][y+1].magnitude)
						tempMap[x][y].magnitude = edgeMatrix[x][y].magnitude;
					break;
				case 3 : 
				case 7 : 
					if(edgeMatrix[x][y].magnitude >= edgeMatrix[x-1][y-1].magnitude && edgeMatrix[x][y].magnitude >= edgeMatrix[x+1][y+1].magnitude)
						tempMap[x][y].magnitude = edgeMatrix[x][y].magnitude;
					break;
 				default: break;
			}
		}
	}

	// Copy temp back to our edge matrix
	for(int y=0; y<edgeMatrix[0].size(); y++)
	{
		for(int x=0; x<edgeMatrix.size(); x++)
		{
			edgeMatrix[x][y].magnitude = tempMap[x][y].magnitude;
		}
	}
}
// Apply double threshold and hysterisis tracking
void Image::threshold(vector<vector<edgePoint>>& edgeMatrix, int threshLow, int threshHigh)
{
	// Double thresholding
	for(int y=0; y<edgeMatrix[0].size(); y++)
	{
		for(int x=0; x<edgeMatrix.size(); x++)
		{
			if(edgeMatrix[x][y].magnitude < threshLow)			// Not an edge
				edgeMatrix[x][y].magnitude = 0;
			else if(edgeMatrix[x][y].magnitude < threshHigh)	// Weak edge
				edgeMatrix[x][y].magnitude = 127;
			else												// Strong edge
				edgeMatrix[x][y].magnitude  = 255;
		}
	}

	// Link the double thresholded values with a silly and inefficient algorithm
	// Iterates through entire matrix until no weak edges neighbor strong ones
	bool more = false;
	do
	{
		more = false;
		for(int y=0; y<edgeMatrix[0].size(); y++)
		{
			for(int x=0; x<edgeMatrix.size(); x++)
			{
				if(edgeMatrix[x][y].magnitude == 127)
				{
					// Top
					if(y>0 && edgeMatrix[x][y-1].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Top right
					else if(y>0 && x<edgeMatrix.size()-1 && edgeMatrix[x+1][y-1].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Right
					else if(x<edgeMatrix.size()-1 && edgeMatrix[x+1][y].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Bottom right
					else if(y<edgeMatrix[0].size()-1 && x<edgeMatrix.size()-1 && edgeMatrix[x+1][y+1].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Bottom
					else if(y<edgeMatrix[0].size()-1 && edgeMatrix[x][y+1].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Bottom left
					else if(y<edgeMatrix[0].size()-1 && x>0 && edgeMatrix[x-1][y+1].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Left
					else if(x>0 && edgeMatrix[x-1][y].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
					// Top left
					else if(y>0 && x>0 && edgeMatrix[x-1][y-1].magnitude == 255)
					{
						edgeMatrix[x][y].magnitude = 255;
						more = true;
					}
				}
			}
		}
	}while(more);

	// Clear all edges not linked to strong edges
	for(int y=0; y<edgeMatrix[0].size(); y++)
	{
		for(int x=0; x<edgeMatrix.size(); x++)
		{
			if(edgeMatrix[x][y].magnitude < 255)
				edgeMatrix[x][y].magnitude = 0;
		}
	}
}
// Merges similar edges into a single edge
void Image::mergeEdges(vector<edge>& edges)
{
	int diag = Common::getDist(point(0,0),point(image->width-1,image->height-1));
	int _ANGLE = 15;			// Max angle between edges
	int _DEGREES = 7;			// Max deviation of connecting edge
	int _DISTANCE = diag/70;	// Max distance to look

	int edgeNum = 0;
	while(edgeNum < edges.size())
	{
		// Vectors of the closest edges
		vector<int> closeEdges;
		vector<int> theCases;

		point curBegin; 
		curBegin.x = edges[edgeNum].points[0].x; 
		curBegin.y = edges[edgeNum].points[0].y;

		point curEnd;   
		curEnd.x = edges[edgeNum].points[edges[edgeNum].points.size()-1].x;
		curEnd.y = edges[edgeNum].points[edges[edgeNum].points.size()-1].y;
	
		// Compare the distance apart for all other edges
		for(int i=edgeNum+1; i<edges.size(); i++)
		{
			point otherBegin;
			otherBegin.x = edges[i].points[0].x;
			otherBegin.y = edges[i].points[0].y;

			point otherEnd;
			otherEnd.x = edges[i].points[edges[i].points.size()-1].x;
			otherEnd.y = edges[i].points[edges[i].points.size()-1].y;

			double a1 = edges[edgeNum].angle, a2 = edges[i].angle;
			if(a1 > 90)
				a1 = 180-a1;
			if(a2 > 90)
				a2 = 180-a2;

			// If the distance is small enough and the angles close enough, note it. Case refers to which two ends are closest
			if(Common::getDist(curBegin, otherBegin) < _DISTANCE && abs(a1 - a2) < _ANGLE)
			{
				closeEdges.push_back(i);
				theCases.push_back(1);
			}
			else if(Common::getDist(curBegin, otherEnd) < _DISTANCE && abs(a1 - a2) < _ANGLE)
			{
				closeEdges.push_back(i);
				theCases.push_back(2);
			}
			else if(Common::getDist(curEnd, otherBegin) < _DISTANCE && abs(a1 - a2) < _ANGLE)
			{
				closeEdges.push_back(i);
				theCases.push_back(3);
			}
			else if(Common::getDist(curEnd, otherEnd) < _DISTANCE && abs(a1 - a2) < _ANGLE)
			{
				closeEdges.push_back(i);
				theCases.push_back(4);
			}
		}

		// Eliminate any edges that run parallel
		for(int i=0; i<closeEdges.size(); i++)
		{
			point p1, p2, p3, p4;

			// Get points between them: p1 and p2 are close, p3 and p4 are far.
			switch(theCases[i])
			{
			case 1 : 
				p1.x = curBegin.x;
				p1.y = curBegin.y;

				p2.x = edges[closeEdges[i]].points[0].x;
				p2.y = edges[closeEdges[i]].points[0].y;

				p3.x = curEnd.x;
				p3.y = curEnd.y;

				p4.x = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].x;
				p4.y = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].y;
				break;
			case 2 : 
				p1.x = curBegin.x;
				p1.y = curBegin.y;

				p2.x = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].x;
				p2.y = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].y;

				p3.x = curEnd.x;
				p3.y = curEnd.y;

				p4.x = edges[closeEdges[i]].points[0].x;
				p4.y = edges[closeEdges[i]].points[0].y;
				break;
			case 3 : 
				p1.x = curEnd.x;
				p1.y = curEnd.y;

				p2.x = edges[closeEdges[i]].points[0].x;
				p2.y = edges[closeEdges[i]].points[0].y;

				p3.x = curBegin.x;
				p3.y = curBegin.y;

				p4.x = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].x;
				p4.y = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].y;
				break;
			case 4 : 
				p1.x = curEnd.x;
				p1.y = curEnd.y;
				
				p2.x = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].x;
				p2.y = edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1].y;

				p3.x = curBegin.x;
				p3.y = curBegin.y;

				p4.x = edges[closeEdges[i]].points[0].x;
				p4.y = edges[closeEdges[i]].points[0].y;
				break;
			}
			
			// Calculate the angles of the lines between the close ends and the far ends
			int betweenAngle = (int)((atan((double)(p1.y-p2.y)/(double)(p1.x-p2.x)) * 180 / _PI)+180)%180;
			int endsAngle = (int)((atan((double)(p3.y-p4.y)/(double)(p3.x-p4.x)) * 180 / _PI)+180)%180;
			int lowAngle, highAngle;

			// Set the minimum and maximum angles
			if(edges[edgeNum].angle < edges[closeEdges[i]].angle)
			{
				lowAngle = edges[edgeNum].angle;
				highAngle = edges[closeEdges[i]].angle;
			}
			else
			{				
				lowAngle = edges[closeEdges[i]].angle;
				highAngle = edges[edgeNum].angle;
			}

			// Move all angles to Q1
			if(lowAngle > 90)
				lowAngle = 180-lowAngle;
			if(highAngle > 90)
				highAngle = 180-highAngle;
			if(betweenAngle > 90)
				betweenAngle = 180-betweenAngle;
			if(endsAngle > 90)
				endsAngle = 180-endsAngle;

			// If the angles are not within range, pop that edge
			if(betweenAngle - lowAngle < -1*_DEGREES || highAngle - betweenAngle < -1*_DEGREES || endsAngle - lowAngle < -1*_DEGREES || highAngle - endsAngle < -1*_DEGREES)
			{
				closeEdges.erase(closeEdges.begin()+i);
				theCases.erase(theCases.begin()+i);
				i--;
			}
		}

		// Merge all specified edges into the current one
		if(closeEdges.size() > 0)
		{
			for(int i=0; i<closeEdges.size(); i++)
			{
				edge tempEdge; // Holds new edge

				switch(theCases[i])
				{
					case 1: // Copy end to beginning, beginning to end
						while(edges[edgeNum].points.size() > 0)
						{
							tempEdge.points.push_back(edges[edgeNum].points[edges[edgeNum].points.size()-1]);
							tempEdge.direction.push_back(edges[edgeNum].direction[edges[edgeNum].direction.size()-1]);

							edges[edgeNum].points.erase(edges[edgeNum].points.begin() + edges[edgeNum].points.size()-1);
							edges[edgeNum].direction.erase(edges[edgeNum].direction.begin() + edges[edgeNum].direction.size()-1);
						}
						while(edges[closeEdges[i]].points.size() > 0)
						{
							tempEdge.points.push_back(edges[closeEdges[i]].points[0]);
							tempEdge.direction.push_back(edges[closeEdges[i]].direction[0]);

							edges[closeEdges[i]].points.erase(edges[closeEdges[i]].points.begin());
							edges[closeEdges[i]].direction.erase(edges[closeEdges[i]].direction.begin());
						}
						break;
					case 2: // Copy end to beginning, end to beginning
						while(edges[edgeNum].points.size() > 0)
						{
							tempEdge.points.push_back(edges[edgeNum].points[edges[edgeNum].points.size()-1]);
							tempEdge.direction.push_back(edges[edgeNum].direction[edges[edgeNum].direction.size()-1]);

							edges[edgeNum].points.erase(edges[edgeNum].points.begin() + edges[edgeNum].points.size()-1);
							edges[edgeNum].direction.erase(edges[edgeNum].direction.begin() + edges[edgeNum].direction.size()-1);
						}
						while(edges[closeEdges[i]].points.size() > 0)
						{
							tempEdge.points.push_back(edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1]);
							tempEdge.direction.push_back(edges[closeEdges[i]].direction[edges[closeEdges[i]].direction.size()-1]);

							edges[closeEdges[i]].points.erase(edges[closeEdges[i]].points.begin() + edges[closeEdges[i]].points.size()-1);
							edges[closeEdges[i]].direction.erase(edges[closeEdges[i]].direction.begin() + edges[closeEdges[i]].direction.size()-1);
						}
						break;
					case 3: // Copy beginning to end, beginning to end
						while(edges[edgeNum].points.size() > 0)
						{
							tempEdge.points.push_back(edges[edgeNum].points[0]);
							tempEdge.direction.push_back(edges[edgeNum].direction[0]);

							edges[edgeNum].points.erase(edges[edgeNum].points.begin());
							edges[edgeNum].direction.erase(edges[edgeNum].direction.begin());
						}
						while(edges[closeEdges[i]].points.size() > 0)
						{
							tempEdge.points.push_back(edges[closeEdges[i]].points[0]);
							tempEdge.direction.push_back(edges[closeEdges[i]].direction[0]);

							edges[closeEdges[i]].points.erase(edges[closeEdges[i]].points.begin());
							edges[closeEdges[i]].direction.erase(edges[closeEdges[i]].direction.begin());
						}
						break;
					case 4: // Copt beginning to end, end to beginning
						while(edges[edgeNum].points.size() > 0)
						{
							tempEdge.points.push_back(edges[edgeNum].points[0]);
							tempEdge.direction.push_back(edges[edgeNum].direction[0]);

							edges[edgeNum].points.erase(edges[edgeNum].points.begin());
							edges[edgeNum].direction.erase(edges[edgeNum].direction.begin());
						}
						while(edges[closeEdges[i]].points.size() > 0)
						{
							tempEdge.points.push_back(edges[closeEdges[i]].points[edges[closeEdges[i]].points.size()-1]);
							tempEdge.direction.push_back(edges[closeEdges[i]].direction[edges[closeEdges[i]].direction.size()-1]);

							edges[closeEdges[i]].points.erase(edges[closeEdges[i]].points.begin() + edges[closeEdges[i]].points.size()-1);
							edges[closeEdges[i]].direction.erase(edges[closeEdges[i]].direction.begin() + edges[closeEdges[i]].direction.size()-1);
						}
						break;
				}
				// Construct new edge
				tempEdge.angle = (int)((atan((double)(tempEdge.points[0].y - tempEdge.points[tempEdge.points.size()-1].y)/(double)(tempEdge.points[0].x - tempEdge.points[tempEdge.points.size()-1].x)) * 180 / _PI)+180)%180;

				// Set edge to new edge
				edges[edgeNum] = tempEdge;
			}

			// Erase the ones merged
			for(int i=closeEdges.size()-1; i>=0; i--)
				edges.erase(edges.begin() + closeEdges[i]);
		}
		else
			edgeNum++;
	}
}
// Starts an edge with point thePoint, and fills it
void Image::pushPoint(int edgeNum, point thePoint, vector<edge>& edges)
{
	double edgeAngle = abs(edgeMap[thePoint.x][thePoint.y].direction-180);

	double _ANGLE = 30;
	int X = thePoint.x;
	int Y = thePoint.y;

	// Push point onto stack
	vector<point> queue;
	queue.push_back(point(X,Y));

	// While stack isn't empty
	while(!queue.empty())
	{
		X = queue[0].x;
		Y = queue[0].y;

		if(edgeMap[X][Y].magnitude == 255)
		{
			// Push to edges
			edges[edgeNum].points.push_back(point(X,Y));
			edges[edgeNum].direction.push_back(edgeMap[X][Y].direction);

			// Set point to black
			edgeMap[X][Y].magnitude = 0;
		}

		// Check 8 connected, push any remaining to stack
		if(Y > 0)
		{
			int nX = X, nY = Y-1;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;

					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Top right
		if(Y > 0 && X < edgeMap.size()-1)
		{
			int nX = X+1, nY = Y-1;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;

					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Right
		if(X < edgeMap.size()-1)
		{
			int nX = X+1, nY = Y;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;

					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Bottom right
		if(X < edgeMap.size()-1 && Y <edgeMap[0].size()-1)
		{
			int nX = X+1, nY = Y+1;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;

					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Bottom
		if(Y <edgeMap[0].size()-1)
		{
			int nX = X, nY = Y+1;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;
					
					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Bottom left
		if(Y <edgeMap[0].size()-1 && X > 0)
		{
			int nX = X-1, nY = Y+1;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;
					
					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Left
		if(X > 0)
		{
			int nX = X-1, nY = Y;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;
					
					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Top left
		if(Y > 0 && X > 0)
		{
			int nX = X-1, nY = Y-1;

			if(edgeMap[nX][nY].magnitude == 255)
			{
				if(abs(edgeAngle - abs(edgeMap[nX][nY].direction-180)) < _ANGLE)
				{
					edges[edgeNum].points.push_back(point(nX,nY));
					edges[edgeNum].direction.push_back(edgeMap[nX][nY].direction);

					// Set point to black
					edgeMap[nX][nY].magnitude = 0;
					
					edgeAngle = (edgeAngle*(edges[edgeNum].points.size()-1) + abs(edgeMap[nX][nY].direction-180))/edges[edgeNum].points.size();

					queue.push_back(point(nX,nY));
				}
			}
		}
		// Pop current from stack
		queue.erase(queue.begin());
	}// End while
}
// Returns a threshold for double thresholding
double Image::getThresh(vector<vector<edgePoint>>& edgeMatrix)
{
	// Create histogram vector
	vector<int> histogram;
	int histSize = (int)pow(2.0,8);
	int maxValue = 0;
	histogram.resize(histSize);

	// Parse image into histogram
	for(int y=0; y<edgeMatrix[0].size(); y++)
	{
		for(int x=0; x<edgeMatrix.size(); x++)
		{
			int v = edgeMatrix[x][y].magnitude;
			if(v<256)
				histogram[v]++;
		}
	}

	for(int j=1; j<histogram.size(); j++)
	{
		if(histogram[j] > maxValue)
			maxValue = histogram[j];
	}

	// Find the threshhold
	double T = histogram.size()/2;
	double Tnew = T;
	double lower=0, upper=0;
	int lowTot=0, highTot=0;
	double difference;

	do
	{
		lower=0; upper=0; lowTot=0; highTot=0;
		// Find average level in lower part
		for(int j=1; j<T; j++)
		{
			lower += histogram[j]*j;
			lowTot += histogram[j];
		}
		lower = lower/lowTot;
		//Find average level in upper part
		for(int j=T; j<histogram.size(); j++)
		{
			upper += histogram[j]*j;
			highTot += histogram[j];
		}
		upper = upper/highTot;
		Tnew = (lower*0.30 + upper*0.70); // 70% of energy below threshold
		difference = T-Tnew;

		T = Tnew;
		
		if(lowTot == 0)
			T = 2;
		else if(highTot == 0)
			T = histogram.size()-2;
	}while(difference != 0 && lowTot != 0 && highTot != 0); // When the threshold is stable, exit

	return T;
}
// Uses binary search to find Rd
double Image::findRad(double diag, double K, double Ru)
{
	double result;
	double lower = 0;
	double upper = diag;
	double Rd = (upper+lower)/2.0;

	// Result should be 0.
	do
	{
		result = Ru - Rd*(1+(K*Rd*Rd));
		if(result > 0)
		{
			lower = Rd;
			Rd = (upper+lower)/2.0;
		}
		else if(result < 0)
		{
			upper = Rd;
			Rd = (upper+lower)/2.0;
		}
	}while(abs(result) > 0.0000001);

	return Rd;
}